/*********************************************************************
 
  (c) copyright Freescale Semiconductor Ltd. 2010
  ALL RIGHTS RESERVED
 
 *********************************************************************
 
 *********************************************************************

  File:				        MMA8452_Read_and_Filter.c
 
  Description:        Read data from MMA8452Q
  
********************************************************************
 Ver 1.0.0	Released Jan.2011
********************************************************************/

/*****************************************************************************
*                                                                           *
* C O N F I D E N T I A L                                                   *
*                                                                           *
* This code is released under the Non-Disclosure Agreement (NDA) between    *
* customer and Freescale, and is allowed to use in Freescale's products     *
* only                                                                      *
*                                                                           *
*****************************************************************************
* THIS  CODE IS ONLY INTENDED AS AN EXAMPLE FOR DEMONSTRATING THE FREESCALE *
* MICROCONTROLLERS.  IT  HAS ONLY BEEN GIVEN A MINIMUM LEVEL OF TEST. IT IS *
* PROVIDED  'AS  SEEN'  WITH  NO  GUARANTEES  AND  NO  PROMISE  OF SUPPORT. *
*****************************************************************************
*                                                                           *
*****************************************************************************
* Freescale  is  not  obligated  to  provide  any  support, upgrades or new *
* releases  of  the Software. Freescale may make changes to the Software at *
* any time, without any obligation to notify or provide updated versions of *
* the  Software  to you. Freescale expressly disclaims any warranty for the *
* Software.  The  Software is provided as is, without warranty of any kind, *
* either  express  or  implied,  including, without limitation, the implied *
* warranties  of  merchantability,  fitness  for  a  particular purpose, or *
* non-infringement.  You  assume  the entire risk arising out of the use or *
* performance of the Software, or any systems you design using the software *
* (if  any).  Nothing  may  be construed as a warranty or representation by *
* Freescale  that  the  Software  or  any derivative work developed with or *
* incorporating  the  Software  will  be  free  from  infringement  of  the *
* intellectual property rights of third parties. In no event will Freescale *
* be  liable,  whether in contract, tort, or otherwise, for any incidental, *
* special,  indirect, consequential or punitive damages, including, but not *
* limited  to,  damages  for  any loss of use, loss of time, inconvenience, *
* commercial loss, or lost profits, savings, or revenues to the full extent *
* such  may be disclaimed by law. The Software is not fault tolerant and is *
* not  designed,  manufactured  or  intended by Freescale for incorporation *
* into  products intended for use or resale in on-line control equipment in *
* hazardous, dangerous to life or potentially life-threatening environments *
* requiring  fail-safe  performance,  such  as  in the operation of nuclear *
* facilities,  aircraft  navigation  or  communication systems, air traffic *
* control,  direct  life  support machines or weapons systems, in which the *
* failure  of  products  could  lead  directly to death, personal injury or *
* severe  physical  or  environmental  damage  (High  Risk Activities). You *
* specifically  represent and warrant that you will not use the Software or *
* any  derivative  work of the Software for High Risk Activities.           *
* Freescale  and the Freescale logos are registered trademarks of Freescale *
* Semiconductor Inc.                                                        *
****************************************************************************/



#include "MMA8452_Read_and_Filter.h"


VUINT8 Rx_Buffer[]; //IIC receiving buffer
VUINT8 Tx_Buffer[]; //IIC trasmitting buffer, first byte is Command

VUINT8 mmaData_Buffer[200]; //Received data buffer


volatile XYZunion8 _XYZdata8;
volatile XYZunion8 _XYZavge8;
volatile XYZunion8 _XYZrsdl8;

//VINT8 Xnew8, Ynew8, Znew8;
VINT16 Xnew16, Ynew16, Znew16;
VINT8 Xdelta8, Ydelta8, Zdelta8;

/********************************************************************************/
/*  Subroutine: Read XYZ 8-bit data                                             */
/********************************************************************************/
/*  argument: VINT8 *x, VINT8 *y, VINT8 *z -- pointers to store read data       */
/*------------------------------------------------------------------------------*/
/*  return:   0 -- IIC failed                                                   */
/*            1 -- IIC succeeded                                                */
/*------------------------------------------------------------------------------*/
/*  global:   IIC reveiving buffer:                                             */
/*            Rx_Buffer[0] -- X8                                                */
/*            Rx_Buffer[1] -- Y8                                                */
/*            Rx_Buffer[2] -- Z8                                                */
/********************************************************************************/

VUINT8 MMA8452_Read_XYZ8(VINT8 *x, VINT8 *y, VINT8 *z){
  if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_CTRL_REG1, &Rx_Buffer[0], 1)) return 0;     //IIC failed
  if(Rx_Buffer[0] & 0x02)
  {
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_X_MSB, &Rx_Buffer[0], 3)) return 0;     //IIC failed
    *x = (VINT8)Rx_Buffer[0];
    *y = (VINT8)Rx_Buffer[1];
    *z = (VINT8)Rx_Buffer[2];
  }
  else
  {
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_X_MSB, &Rx_Buffer[0], 6)) return 0;     //IIC failed
    *x = (VINT8)Rx_Buffer[0];
    *y = (VINT8)Rx_Buffer[2];
    *z = (VINT8)Rx_Buffer[4];
  }
  return 1;
}

/********************************************************************************/
/*  Subroutine: Read XYZ 14-bit data                                            */
/********************************************************************************/
/*  argument: VINT8 *x, VINT8 *y, VINT8 *z -- pointers to store read data       */
/*------------------------------------------------------------------------------*/
/*  return:   0 -- IIC failed                                                   */
/*            1 -- IIC succeeded                                                */
/*------------------------------------------------------------------------------*/
/*  global:   IIC reveiving buffer:                                             */
/********************************************************************************/

VUINT8 MMA8452_Read_XYZ12(VINT16 *x, VINT16 *y, VINT16 *z){
  VUINT8 temp8u;

  if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_CTRL_REG1, &Rx_Buffer[0], 1)) return 0;     //IIC failed
  if(!(Rx_Buffer[0] & 0x02))
  {
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_X_MSB, &Rx_Buffer[0], 6)) return 0;    //IIC failed
    *x = ((((VINT16)Rx_Buffer[0])<<4)&0x0FF0) | ((((VINT16)Rx_Buffer[1])>>4)&0x000F);
    if(*x&0x0800) *x |= 0xF000;
    *y = ((((VINT16)Rx_Buffer[2])<<4)&0x0FF0) | ((((VINT16)Rx_Buffer[3])>>4)&0x000F);
    if(*y&0x0800) *y |= 0xF000;
    *z = ((((VINT16)Rx_Buffer[4])<<4)&0x0FF0) | ((((VINT16)Rx_Buffer[5])>>4)&0x000F);
    if(*z&0x0800) *z |= 0xF000;
  }
  else
  {
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_X_MSB, &Rx_Buffer[0], 3)) return 0;    //IIC failed
    *x = ((((VINT16)Rx_Buffer[0])<<4)&0x0FF0);
    *y = ((((VINT16)Rx_Buffer[1])<<4)&0x0FF0);
    *z = ((((VINT16)Rx_Buffer[2])<<4)&0x0FF0);
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_X_LSB, &Rx_Buffer[0], 1)) return 0;    //IIC failed
    *x |= (((VINT16)Rx_Buffer[0])>>4)&0x000F;
    if(*x&0x0800) *x |= 0xF000;
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_Y_LSB, &Rx_Buffer[0], 1)) return 0;    //IIC failed
    *y |= (((VINT16)Rx_Buffer[0])>>4)&0x000F;
    if(*y&0x0800) *y |= 0xF000;
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_Z_LSB, &Rx_Buffer[0], 1)) return 0;    //IIC failed
    *z |= (((VINT16)Rx_Buffer[0])>>4)&0x000F;
    if(*z&0x0800) *z |= 0xF000;
  }
  return 1;
}

/********************************************************************************/
/*  Subroutine: Start and Stop the Sensor                                       */
/********************************************************************************/
/*  argument: 0 -- stop the sensor                                              */
/*            1 -- start to 2g mode                                             */
/*            2 -- start to 4g mode                                             */
/*            3 -- start to 8g mode                                             */
/*            ASLP_RATE and DR could be set at the same time, default all 0     */
/*------------------------------------------------------------------------------*/
/*  return:   0 -- IIC failed                                                   */
/*            1 -- IIC succeeded                                                */
/*------------------------------------------------------------------------------*/
/*  global:   none                                                              */
/********************************************************************************/

VUINT8 FSL_MMA_StStp(VUINT8 reg_value){
  if(reg_value)
  {
    if(!FSL_MMA_IICWrite(MMA8452_CTRL_REG1, 0)) return 0;   //IIC failed
    if(!FSL_MMA_IICWrite(MMA8452_CTRL_REG1, (reg_value)&0xFE)) return 0;  //IIC failed
    return (FSL_MMA_IICWrite(MMA8452_CTRL_REG1, reg_value));
  }
  else
  {
    if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_CTRL_REG1, &Rx_Buffer[0], 1)) return 0;    //IIC failed
    return(FSL_MMA_IICWrite(MMA8452_CTRL_REG1, (Rx_Buffer[0]&0xFE)));
  }
}

/********************************************************************************/
/*  Subroutine: Reset MMA8452Q                                                  */
/********************************************************************************/
/*  argument: none                                                              */
/*------------------------------------------------------------------------------*/
/*  return:   0 -- IIC failed                                                   */
/*            1 -- IIC succeeded                                                */
/*------------------------------------------------------------------------------*/
/*  global:   none                                                              */
/********************************************************************************/

VUINT8 MMA8452_Reset(void){
  return (FSL_MMA_IICWrite(MMA8452_CTRL_REG2, 0x40));
}

/********************************************************************************/
/*  Subroutine: Start Up Delay after Starting up                                */
/********************************************************************************/
/*  argument: none                                                              */
/*------------------------------------------------------------------------------*/
/*  return:   none                                                              */
/*------------------------------------------------------------------------------*/
/*  global:   none                                                              */
/********************************************************************************/

void FSL_MMA_Startup_Delay(){
// MMA8452Q Must delay 1.5mS after start up by CS pin  

//===============================================================================================
// For Debug
//===============================================================================================
//    DisplayString("\r\n*** Sensor starts up!\n\r");
//===============================================================================================
// Debug End
//===============================================================================================
}

/********************************************************************************/
/*  Subroutine: Check wheter new data is ready                                  */
/********************************************************************************/
/*  argument: none                                                              */
/*------------------------------------------------------------------------------*/
/*  return:   0 - data is not ready                                             */
/*            1 - data is ready                                                 */
/*------------------------------------------------------------------------------*/
/*  global:   none                                                              */
/********************************************************************************/

VUINT8 MMA8452_Data_Is_Ready(void){
  if(!FSL_MMA_IIC_ReadDataBlock(MMA8452_STATUS1, &Rx_Buffer[0], 1)) return 0;    //IIC failed
  if((Rx_Buffer[0] & 0x07) == 0x07) return 1;
  return 0;
}


/********************************************************************************/
/*  Subroutine: 8-Bit Data Low Pass Filter                                      */
/********************************************************************************/
/*  argument: VINT8 * InData - Input data address                               */
/*            VINT8 * OutData - Output data address                             */
/*            VUINT8 Factor - Low Pass Factor                                   */
/*------------------------------------------------------------------------------*/
/*  return:   none                                                              */
/*------------------------------------------------------------------------------*/
/*  global:   none                                                              */
/********************************************************************************/

void FSL_MMA_LowPassFilter_8(VINT8 * InData, VINT8 * OutData, VINT8 * RsdlData, VUINT8 Factor){
  VINT16 temp16s1;

#if 1
  VINT8 temp8s1;
  VUINT8 i;
  
  for(i=0; i<3; i++)
  {
    temp16s1 = (VINT16)InData[i] + (VINT16)(Factor*OutData[i]);
    temp8s1 = (VINT8)(temp16s1/(1+Factor));
    RsdlData[i] += (VINT8)(temp16s1 - (VINT16)(temp8s1*(1 + Factor)));
    if(temp8s1==OutData[i])
    {
      if(abs(RsdlData[i])>((1+Factor)))
      {
        if(RsdlData[i]>0) OutData[i]++;
        else OutData[i]--;
        RsdlData[i] = 0;
      }
    }
    else
    {
#if 1
        OutData[i] = temp8s1;
        RsdlData[i] = 0;
#else
      if((abs(temp8s1)>abs(OutData[i]))||(abs(temp8s1-OutData[i])>1))
      {
        OutData[i] = temp8s1;
        RsdlData[i] = 0;
      }else{
        if(abs(RsdlData[i])>((1+Factor)))
        {
          if(RsdlData[i]>0) OutData[i]--;
          else OutData[i]++;
          RsdlData[i] = 0;
        }
      }
#endif
    }
  }

 #else

  VINT8 temp8s1;

  temp8s1 = OutData[0];
  temp16s1 = (VINT16)InData[0] + (VINT16)(Factor*OutData[0]);
  OutData[0] = (VINT8)(temp16s1/(1 + Factor));
  if(temp8s1==OutData[0])
  {
    if(InData[0]>OutData[0]) OutData[0]++;
    else if(InData[0]<OutData[0]) OutData[0]--;
  }

  temp8s1 = OutData[1];
  temp16s1 = (VINT16)InData[1] + (VINT16)(Factor*OutData[1]);
  OutData[1] = (VINT8)(temp16s1/(1 + Factor));
  if(temp8s1==OutData[1])
  {
    if(InData[1]>OutData[1]) OutData[1]++;
    else if(InData[1]<OutData[1]) OutData[1]--;
  }

  temp8s1 = OutData[2];
  temp16s1 = (VINT16)InData[2] + (VINT16)(Factor*OutData[2]);
  OutData[2] = (VINT8)(temp16s1/(1 + Factor));
  if(temp8s1==OutData[2])
  {
    if(InData[2]>OutData[2]) OutData[2]++;
    else if(InData[2]<OutData[2]) OutData[2]--;
  }

 #endif
}

/************************************************/
/************************************************/
